#include "interface_uart.hpp"
#include "iMath.h"
#include "polyfit.h"
#include "strutil.hpp"
#include "task_main.h"
#include "arm.hpp"
#include "Stepper.hpp"
#include "Stepper_driver.h"
#include "board_base.h"
#include "track_config.hpp"
// C++
extern Stepper stepper1;
extern Stepper stepper2;

// Line_polyfit2_t pf2_y = {-0.05253, -0.1964, -1.705, 22.99, 524.7, -1.399e+04};

double tempStepX=0,tempStepY=0;
uint8_t MCSU_Buf[10]; // 解析器的缓存,Split分割后的数据的引索存储到这里
StrUtil MCSU("MC:",',',";",MCSU_Buf,sizeof(MCSU_Buf),NULL,0);
StrUtil ARMSU("ARM:",',',";",MCSU_Buf,sizeof(MCSU_Buf),NULL,0);
StrUtil CAMSU("Cam:",',',";",MCSU_Buf,sizeof(MCSU_Buf),NULL,0);
void motor_interface(uint8_t* pdata, int size)
{
    char *pstr;
    Stepper *motor;
    MCSU.setSrcAddress(pdata, size);
    if (MCSU.parse() == 0) return;
    switch (MCSU.getInt(0)) // 第一个参数用来判断控制的对象
    {
    case 1:
        motor = &stepper1;
        break;
    case 2:
        motor = &stepper2;
        break;
    default :
        return;
    }
	pstr = MCSU.getStr(1);
    switch (*pstr)
    {
    case 'r': //reset
        motor->reset(-1);
        break;
    case 'a': //angle
        motor->rotateToAngle((float)MCSU.getDouble(2)); // MC:1,a,90;
        break;
    case 's': //step/stop
        if (MCSU.strCmp(1, "step"))
            motor->moveToStep((long long)MCSU.getInt(2));
        if (MCSU.strCmp(1, "stop"))
            motor->stop();
        break;
    case 'v': //velocity
        motor->setSpeed(MCSU.getDouble(2));
        break;
    case 'm': //move angle/step
        pstr++;
        if (*pstr == 'a')
        {
            motor->rotateAngle((float)MCSU.getDouble(2));
        }
        else if (*pstr == 's') //MC:1,s,steps
        {
            motor->moveStep((long)MCSU.getInt(2));
        }
        break;
    case 'u': //unlimit
        if(MCSU.getInt(2)) motor->limitStepEnable();
        else motor->limitStepDisable();
        break;
    case 'c': //clear Cnt
        motor->driver.stepCnt = 0;
        break;
    }
}
int CaliCnt = 0;
void arm_interface(uint8_t *pdata, int size)
{
    char *pstr;
    ARMSU.setSrcAddress(pdata, size);
    if (ARMSU.parse() == 0)
    {
        return;
    }
    
    pstr = ARMSU.getStr(0);
    
    switch (*pstr)
    {
    case 'r':
        if (ARMSU.strCmp(0, "reset"))
        {
            Arm.stop();
            Arm.startReset();
        }
        else if (ARMSU.strCmp(0, "resetV"))
        {
            Arm.configResetSpeed((float)ARMSU.getDouble(1),(float)ARMSU.getDouble(2));
        }
        else if (ARMSU.strCmp(0, "resetOffset"))
        {
            Arm.configResetOffset((float)ARMSU.getDouble(1),(float)ARMSU.getDouble(2));
        }
        else if (ARMSU.strCmp(0, "resetCnt"))
        {
            Arm.configResetCnt(ARMSU.getInt(1));
        }
        break;
    case 'a':
        Arm.startGoAim(ARMSU.getInt(1), ARMSU.getInt(2)); // ARM:a,x,y;
        break;
    case 'v':
        Arm.configGoAimSpeed(ARMSU.getInt(1), ARMSU.getInt(2));
        break;
    case 's':
        if (ARMSU.strCmp(0, "setX"))
        {
            tempStepX = ARMSU.getInt(1);
        }
        else if(ARMSU.strCmp(0, "setY"))
        {
            tempStepY = ARMSU.getInt(1);
        }
        else if (ARMSU.strCmp(0, "stop"))
        {
            Arm.stop();
        }
        break;
    case 'g':
        Arm.startGoAimCoodinate(tempStepX,tempStepY);
        break;
    case 'i':
        Beep(10);
        if (ARMSU.strCmp(1, "add"))
        {
            Queue_Printf(&htx1, "[%d]Arm info:%lld,%lld,%.02lf,%.02lf\r\n", CaliCnt++, Arm.stepperX.getStep(), Arm.stepperY.getStep(),Arm.stepperX.getAngle(),Arm.stepperY.getAngle());
        }
        else if (ARMSU.strCmp(1, "minus"))
        {
            Queue_Printf(&htx1, "Arm CaliCnt back to [%d] \r\n", --CaliCnt);
        }
        else if (ARMSU.strCmp(1, "reset"))
        {
            Queue_Printf(&htx1, "Arm CaliCnt reset\r\n");
            CaliCnt = 0;
        }
        break;
    }
}



extern trackResultCollecter trackReceive;
void camera_interface(uint8_t *pdata, int size)
{
    char *pstr;
    CAMSU.setSrcAddress(pdata, size);
    if (CAMSU.parse() == 0)
    {
        return;
    }
    pstr = CAMSU.getStr(0);
    switch (*pstr)
    {
    case 'i'://info
        trackResult.color = CAMSU.getInt(1);
        trackResult.sx = CAMSU.getInt(2);
        trackResult.sy = CAMSU.getInt(3);
        trackResult.w = CAMSU.getInt(4);
        trackResult.h = CAMSU.getInt(5);
        trackResult.solidity = (float)CAMSU.getDouble(6);
        trackReceive.addResult(&trackResult);
        break;
    case 'g'://get
        if (CAMSU.strCmp(0, "get"))
        {
            // trackReceive.print(&trackResult);
            trackReceive.printResults();
        }
        else if (CAMSU.strCmp(0, "go"))
        {
            // camAxis2step(trackResult.sx + (trackResult.w>>1), trackResult.sy + (trackResult.h>>1));
            // Queue_Printf(&htx1, "aim:%s,%d,%d,%d,%d,%s;\n\r",
            //             trackObjColor[trackResult.color],
            //             trackResult.sx, trackResult.sy,
            //             trackResult.w, trackResult.h,
            //             trackObjShape[trackResult.shape],
            //             Arm.goaimConfig.stepX, Arm.goaimConfig.stepY);
            // Arm.trackObject(trackReceive);
            if (CAMSU.BufUsage == 1)
                startColorTrack(0, &trackconfig);
            else
            {
                Arm.trackObject(trackReceive.readResult(CAMSU.getInt(1)));
            }
        }
        break;
    case 's'://set
        if (CAMSU.strCmp(0, "set"))
        {
            pstr = CAMSU.getStr(1);
            switch (*pstr)
            {
            case 'a':
                if (CAMSU.strCmp(1, "all"))
                {
                    trackconfig.color = CAMSU.getInt(2);
                    trackconfig.shape = CAMSU.getInt(3);
                    trackconfig.xSizeMin = CAMSU.getInt(4);
                    trackconfig.xSizeMax = CAMSU.getInt(5);
                    trackconfig.ySizeMin = CAMSU.getInt(6);
                    trackconfig.ySizeMax = CAMSU.getInt(7);
                }
                break;
            case 'c':
                if (CAMSU.strCmp(1, "color"))
                {
                    trackconfig.color = CAMSU.getInt(2);
                }
                break;
            case 's':
                if (CAMSU.strCmp(1, "shape"))
                {
                    trackconfig.shape = CAMSU.getInt(2);
                }
                break;
            case 'x':
                if (CAMSU.strCmp(1, "xSize")) //xMin,xMax
                {
                    trackconfig.xSizeMin = CAMSU.getInt(2);
                    trackconfig.xSizeMax = CAMSU.getInt(3);
                }
                break;
            case 'y':
                if (CAMSU.strCmp(1, "ySize"))//yMin,yMax
                {
                    trackconfig.ySizeMin = CAMSU.getInt(2);
                    trackconfig.ySizeMax = CAMSU.getInt(3);
                }
                break;
            default:
                break;
            }
        }
        break;
    case 'c':
        if (CAMSU.strCmp(0, "clear"))
        {
            trackReceive.clearResult();
        }
        break;
    default:
        break;
    }
}
